#include "auto_navigation/planning/pointcloud_planner.hpp"
#include <cmath>
#include <algorithm>
#include <tf/transform_datatypes.h>

namespace auto_navigation {
namespace planning {

PointCloudPlanner::PointCloudPlanner()
    : PlannerInterface("pointcloud_planner"),
      cloud_(new pcl::PointCloud<pcl::PointXYZ>()),
      octree_(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(0.1)),
      cloud_received_(false),
      resolution_(0.2),
      min_height_(-0.5),
      max_height_(0.5),
      obstacle_radius_(0.3),
      width_(0),
      height_(0),
      origin_x_(0.0),
      origin_y_(0.0) {
}

PointCloudPlanner::~PointCloudPlanner() {
}

bool PointCloudPlanner::initialize(ros::NodeHandle& nh) {
    // 从参数服务器获取参数
    nh.param<double>("pointcloud_planner/resolution", resolution_, 0.2);
    nh.param<double>("pointcloud_planner/min_height", min_height_, -0.5);
    nh.param<double>("pointcloud_planner/max_height", max_height_, 0.5);
    nh.param<double>("pointcloud_planner/obstacle_radius", obstacle_radius_, 0.3);
    
    ROS_INFO("PointCloudPlanner initialized with resolution: %.2f, height range: [%.2f, %.2f], obstacle radius: %.2f",
             resolution_, min_height_, max_height_, obstacle_radius_);
    
    return true;
}

bool PointCloudPlanner::setMap(const nav_msgs::OccupancyGrid& map) {
    // 这个方法不会被使用，但需要实现以满足接口要求
    ROS_WARN("PointCloudPlanner::setMap called with OccupancyGrid, but this planner uses PointCloud2 data");
    return false;
}

bool PointCloudPlanner::setPointCloud(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
    ROS_INFO("PointCloudPlanner::setPointCloud - Starting with cloud_received_: %d", cloud_received_);
    
    // 将ROS消息转换为PCL点云
    pcl::fromROSMsg(*cloud_msg, *cloud_);
    
    // 更新八叉树
    octree_->setInputCloud(cloud_);
    octree_->addPointsFromInputCloud();
    
    // 计算点云的边界
    double min_x = std::numeric_limits<double>::max();
    double min_y = std::numeric_limits<double>::max();
    double max_x = std::numeric_limits<double>::lowest();
    double max_y = std::numeric_limits<double>::lowest();
    
    for (const auto& point : cloud_->points) {
        if (point.z >= min_height_ && point.z <= max_height_) {
            min_x = std::min(min_x, static_cast<double>(point.x));
            min_y = std::min(min_y, static_cast<double>(point.y));
            max_x = std::max(max_x, static_cast<double>(point.x));
            max_y = std::max(max_y, static_cast<double>(point.y));
        }
    }
    
    // 添加边界缓冲区
    min_x -= 5.0;
    min_y -= 5.0;
    max_x += 5.0;
    max_y += 5.0;
    
    // 设置栅格地图的原点和尺寸
    origin_x_ = min_x;
    origin_y_ = min_y;
    width_ = static_cast<int>((max_x - min_x) / resolution_) + 1;
    height_ = static_cast<int>((max_y - min_y) / resolution_) + 1;
    
    ROS_INFO("Created grid map with size %d x %d at origin (%.2f, %.2f)",
             width_, height_, origin_x_, origin_y_);
    
    // 创建2D栅格地图
    createGridMap();
    
    cloud_received_ = true;
    ROS_INFO("PointCloudPlanner::setPointCloud - Finished with cloud_received_: %d, cloud size: %lu", 
            cloud_received_, cloud_->points.size());
    return true;
}

void PointCloudPlanner::createGridMap() {
    // 初始化栅格地图
    grid_map_.resize(width_, std::vector<bool>(height_, false));
    
    // 遍历点云中的点，标记占用的栅格
    for (const auto& point : cloud_->points) {
        // 只考虑指定高度范围内的点
        if (point.z >= min_height_ && point.z <= max_height_) {
            int gx, gy;
            if (worldToGrid(point.x, point.y, gx, gy)) {
                // 标记该点为占用
                grid_map_[gx][gy] = true;
                
                // 膨胀障碍物
                int radius_cells = static_cast<int>(obstacle_radius_ / resolution_);
                for (int dx = -radius_cells; dx <= radius_cells; ++dx) {
                    for (int dy = -radius_cells; dy <= radius_cells; ++dy) {
                        if (dx*dx + dy*dy <= radius_cells*radius_cells) {
                            int nx = gx + dx;
                            int ny = gy + dy;
                            if (isValidCell(nx, ny)) {
                                grid_map_[nx][ny] = true;
                            }
                        }
                    }
                }
            }
        }
    }
}

bool PointCloudPlanner::worldToGrid(double wx, double wy, int& gx, int& gy) const {
    gx = static_cast<int>((wx - origin_x_) / resolution_);
    gy = static_cast<int>((wy - origin_y_) / resolution_);
    
    return isValidCell(gx, gy);
}

void PointCloudPlanner::gridToWorld(int gx, int gy, double& wx, double& wy) const {
    wx = origin_x_ + gx * resolution_ + resolution_ / 2.0;
    wy = origin_y_ + gy * resolution_ + resolution_ / 2.0;
}

bool PointCloudPlanner::isValidCell(int x, int y) const {
    return x >= 0 && x < width_ && y >= 0 && y < height_;
}

bool PointCloudPlanner::isOccupied(int x, int y) const {
    if (!isValidCell(x, y)) {
        return true;  // 将地图外的区域视为占用
    }
    return grid_map_[x][y];
}

double PointCloudPlanner::heuristic(int x1, int y1, int x2, int y2) const {
    // 使用欧几里得距离作为启发式函数
    return std::sqrt(std::pow(x2 - x1, 2) + std::pow(y2 - y1, 2));
}

bool PointCloudPlanner::plan(const geometry_msgs::PoseStamped& start,
                           const geometry_msgs::PoseStamped& goal,
                           nav_msgs::Path& path) {
    // 添加详细的调试信息
    ROS_INFO("PointCloudPlanner::plan - cloud_received_: %d, cloud size: %lu, width: %d, height: %d",
             cloud_received_, cloud_->points.size(), width_, height_);
             
    if (!cloud_received_) {
        ROS_ERROR("PointCloudPlanner: No point cloud map received yet");
        return false;
    }
    
    if (cloud_->points.empty()) {
        ROS_ERROR("PointCloudPlanner: Point cloud is empty");
        return false;
    }
    
    if (width_ <= 0 || height_ <= 0) {
        ROS_ERROR("PointCloudPlanner: Invalid grid dimensions: %dx%d", width_, height_);
        return false;
    }
    
    // 将起点和终点转换为栅格坐标
    int start_x, start_y, goal_x, goal_y;
    if (!worldToGrid(start.pose.position.x, start.pose.position.y, start_x, start_y)) {
        ROS_ERROR("PointCloudPlanner: Start position (%.2f, %.2f) is outside the map",
                 start.pose.position.x, start.pose.position.y);
        return false;
    }
    
    if (!worldToGrid(goal.pose.position.x, goal.pose.position.y, goal_x, goal_y)) {
        ROS_ERROR("PointCloudPlanner: Goal position (%.2f, %.2f) is outside the map",
                 goal.pose.position.x, goal.pose.position.y);
        return false;
    }
    
    ROS_INFO("PointCloudPlanner: Planning from grid (%d, %d) to grid (%d, %d)",
             start_x, start_y, goal_x, goal_y);
             
    // 检查起点和终点是否被占用
    if (isOccupied(start_x, start_y)) {
        ROS_ERROR("PointCloudPlanner: Start position is occupied");
        return false;
    }
    
    if (isOccupied(goal_x, goal_y)) {
        ROS_ERROR("PointCloudPlanner: Goal position is occupied");
        return false;
    }
    
    // A*算法实现
    std::priority_queue<std::pair<double, GridCell>,
                        std::vector<std::pair<double, GridCell>>,
                        std::greater<std::pair<double, GridCell>>> open_set;
    
    std::unordered_map<GridCell, double, GridCellHash> g_score;
    std::unordered_map<GridCell, GridCell, GridCellHash> came_from;
    
    GridCell start_cell = {start_x, start_y};
    GridCell goal_cell = {goal_x, goal_y};
    
    g_score[start_cell] = 0;
    open_set.push(std::make_pair(heuristic(start_x, start_y, goal_x, goal_y), start_cell));
    
    // 定义8个方向的移动
    const int dx[8] = {-1, -1, -1, 0, 0, 1, 1, 1};
    const int dy[8] = {-1, 0, 1, -1, 1, -1, 0, 1};
    
    while (!open_set.empty()) {
        GridCell current = open_set.top().second;
        open_set.pop();
        
        // 到达目标
        if (current.x == goal_cell.x && current.y == goal_cell.y) {
            reconstructPath(came_from, current, path);
            return true;
        }
        
        // 探索邻居
        for (int i = 0; i < 8; ++i) {
            GridCell neighbor = {current.x + dx[i], current.y + dy[i]};
            
            if (!isValidCell(neighbor.x, neighbor.y) || isOccupied(neighbor.x, neighbor.y)) {
                continue;
            }
            
            // 计算移动代价（对角线移动代价为1.414，直线移动代价为1）
            double move_cost = (dx[i] != 0 && dy[i] != 0) ? 1.414 : 1.0;
            double tentative_g_score = g_score[current] + move_cost;
            
            if (g_score.find(neighbor) == g_score.end() || tentative_g_score < g_score[neighbor]) {
                came_from[neighbor] = current;
                g_score[neighbor] = tentative_g_score;
                double f_score = tentative_g_score + heuristic(neighbor.x, neighbor.y, goal_x, goal_y);
                open_set.push(std::make_pair(f_score, neighbor));
            }
        }
    }
    
    ROS_ERROR("PointCloudPlanner: No path found from start to goal");
    return false;
}

void PointCloudPlanner::reconstructPath(
    const std::unordered_map<GridCell, GridCell, GridCellHash>& came_from,
    const GridCell& current,
    nav_msgs::Path& path) const {
    
    std::vector<GridCell> reversed_path;
    GridCell cell = current;
    
    // 从目标回溯到起点
    while (came_from.find(cell) != came_from.end()) {
        reversed_path.push_back(cell);
        cell = came_from.at(cell);
    }
    reversed_path.push_back(cell);  // 添加起点
    
    // 反转路径并转换为ROS路径消息
    path.header.frame_id = "map";
    path.header.stamp = ros::Time::now();
    path.poses.clear();
    
    for (auto it = reversed_path.rbegin(); it != reversed_path.rend(); ++it) {
        geometry_msgs::PoseStamped pose;
        pose.header = path.header;
        
        double wx, wy;
        gridToWorld(it->x, it->y, wx, wy);
        
        pose.pose.position.x = wx;
        pose.pose.position.y = wy;
        pose.pose.position.z = 0.0;
        
        // 设置方向（如果不是最后一个点）
        if (it != reversed_path.rend() - 1) {
            auto next = it + 1;
            double next_wx, next_wy;
            gridToWorld(next->x, next->y, next_wx, next_wy);
            
            double yaw = std::atan2(next_wy - wy, next_wx - wx);
            pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
        } else {
            // 最后一个点使用默认方向
            pose.pose.orientation.w = 1.0;
        }
        
        path.poses.push_back(pose);
    }
    
    ROS_INFO("Path found with %zu waypoints", path.poses.size());
}

} // namespace planning
} // namespace auto_navigation 